#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>
#include "crc8_crc16.h"
#include <sensor_msgs/Imu.h>

float vx = 0.0f;
float vy = 0.0f;
float vth = 0.0f;
float ax = 0.0f;
float ay = 0.0f;
float wz = 0.0f;

typedef enum
{
	VX = 0,
	VY,
	VTH,
	AX,
	AY,
	WZ,
	ODOM_Data_Total,
} odom_data_e;

typedef union
{
	unsigned char c[4];
	float f;
} vel_cmd_data_u;

vel_cmd_data_u odom_data[ODOM_Data_Total];

double ODOM_POSE_COVARIANCE[6][6] = {1e-3, 0, 0, 0, 0, 0,
									 0, 1e-3, 0, 0, 0, 0,
									 0, 0, 1e6, 0, 0, 0,
									 0, 0, 0, 1e6, 0, 0,
									 0, 0, 0, 0, 1e6, 0,
									 0, 0, 0, 0, 0, 1e3};
double ODOM_POSE_COVARIANCE2[6][6] = {1e-9, 0, 0, 0, 0, 0,
									  0, 1e-3, 1e-9, 0, 0, 0,
									  0, 0, 1e6, 0, 0, 0,
									  0, 0, 0, 1e6, 0, 0,
									  0, 0, 0, 0, 1e6, 0,
									  0, 0, 0, 0, 0, 1e-9};

double ODOM_TWIST_COVARIANCE[6][6] = {1e-3, 0, 0, 0, 0, 0,
									  0, 1e-3, 0, 0, 0, 0,
									  0, 0, 1e6, 0, 0, 0,
									  0, 0, 0, 1e6, 0, 0,
									  0, 0, 0, 0, 1e6, 0,
									  0, 0, 0, 0, 0, 1e3};
double ODOM_TWIST_COVARIANCE2[6][6] = {1e-9, 0, 0, 0, 0, 0,
									   0, 1e-3, 1e-9, 0, 0, 0,
									   0, 0, 1e6, 0, 0, 0,
									   0, 0, 0, 1e6, 0, 0,
									   0, 0, 0, 0, 1e6, 0,
									   0, 0, 0, 0, 0, 1e-9};

void str2odometer_callback(const std_msgs::String chassis_msg)
{
	static int rev_cnt = 0;
	static int tv = clock();
	uint8_t cha_RxCnt = 0;
	uint8_t buf[5 + ODOM_Data_Total * 4];
	memset(buf, 0, sizeof(buf));
	rev_cnt++;
	//接受信息
	// ROS_INFO(" start receive data:%s\n", chassis_msg.data.c_str());
	//帧头判断
	if (cha_RxCnt == 0 && 0xA5 == chassis_msg.data[0])
	{
		buf[0] = chassis_msg.data[cha_RxCnt++];
	}
	if (cha_RxCnt == 1 && 0x5A == chassis_msg.data[1])
	{
		buf[1] = chassis_msg.data[cha_RxCnt++];
	}
	if (cha_RxCnt >= 2 && cha_RxCnt < (5 + ODOM_Data_Total * 4))
	{
		for (int i = 2; i < 5 + ODOM_Data_Total * 4; ++i)
		{
			if (cha_RxCnt < 2 + ODOM_Data_Total * 4)
				buf[i] = chassis_msg.data[cha_RxCnt++];
			else
			{
				//结束位与校验位判断
				if (cha_RxCnt == (2 + ODOM_Data_Total * 4) && 0xFE == chassis_msg.data[cha_RxCnt])
				{
					buf[i] = chassis_msg.data[cha_RxCnt++];
					//进入校验码判断
					append_CRC16_check_sum(buf, 17);
				}
				else if (cha_RxCnt == (3 + ODOM_Data_Total * 4) && buf[(3 + ODOM_Data_Total * 4)] == chassis_msg.data[cha_RxCnt])
				{
					buf[i] = chassis_msg.data[cha_RxCnt++];
				}
				else if (cha_RxCnt == (4 + ODOM_Data_Total * 4) && buf[(4 + ODOM_Data_Total * 4)] == chassis_msg.data[cha_RxCnt])
				{
					buf[i] = chassis_msg.data[cha_RxCnt++];
				}
			}
		}
		if (cha_RxCnt == (5 + ODOM_Data_Total * 4))
		{
			//通信协议，把字符串里的值依次赋值,单片机大疆C板,jetson nano均为小端模式
			for (int i = 0, k = 2; i < ODOM_Data_Total; ++i)
			{
				for (int j = 0; j < 4; ++j)
					odom_data[i].c[j] = buf[k++];
			}

			vx = odom_data[VX].f;

			vy = odom_data[VY].f;

			vth = odom_data[VTH].f;

			ax = odom_data[AX].f;

			ay = odom_data[AY].f;

			wz = odom_data[WZ].f;

			cha_RxCnt = 0;
			// ROS_INFO("the %i time success, cost %ims\n",rev_cnt,clock()-tv);
			ROS_INFO("successfully receive a frame of data\n");

			ROS_INFO("chassis_odom info:vx:%f, vy:%f, vth:%f, ax:%f, ay:%f, wz:%f\n", vx, vy, vth, ax, ay, wz);
		}
		else
		{
			cha_RxCnt = 0;
		}
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "odometry_publisher");

	ros::NodeHandle n;
	ros::Subscriber write_sub = n.subscribe("r2_chassis", 1000, str2odometer_callback);
	ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 100);
	ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("/imu_data", 100);
	tf::TransformBroadcaster odom_broadcaster;

	double x = 0.0;
	double y = 0.0;
	double th = 0.0;

	double ang_wz = 0.0;

	ros::Time current_time, last_time;
	current_time = ros::Time::now();
	last_time = ros::Time::now();

	ros::Rate r(200);
	while (n.ok())
	{
		ros::spinOnce(); // check for incoming messages
		current_time = ros::Time::now();

		// compute odometry in a typical way given the velocities of the robot
		double dt = (current_time - last_time).toSec();
		double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
		double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
		double delta_th = vth * dt;

		x += delta_x;
		y += delta_y;
		th += delta_th;

		// since all odometry is 6DOF we'll need a quaternion created from yaw
		geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

		// // first, we'll publish the transform over tf
		// //由robot_pose_ekf发布
		// geometry_msgs::TransformStamped odom_trans;
		// odom_trans.header.stamp = current_time;
		// odom_trans.header.frame_id = "odom";
		// odom_trans.child_frame_id = "base_link";

		// odom_trans.transform.translation.x = x;
		// odom_trans.transform.translation.y = y;
		// odom_trans.transform.translation.z = 0.0;
		// odom_trans.transform.rotation = odom_quat;

		// // send the transform
		// odom_broadcaster.sendTransform(odom_trans);

		// next, we'll publish the odometry message over ROS
		nav_msgs::Odometry odom;
		odom.header.stamp = current_time;
		odom.header.frame_id = "/odom";

		// set the position
		odom.pose.pose.position.x = x;
		odom.pose.pose.position.y = y;
		odom.pose.pose.position.z = 0.0;
		odom.pose.pose.orientation = odom_quat;
		odom.pose.covariance = {1e-3, 0, 0, 0, 0, 0,
								0, 1e-3, 0, 0, 0, 0,
								0, 0, 1e6, 0, 0, 0,
								0, 0, 0, 1e6, 0, 0,
								0, 0, 0, 0, 1e6, 0,
								0, 0, 0, 0, 0, 1e3};

		// set the velocity
		odom.child_frame_id = "base_link";
		odom.twist.twist.linear.x = vx;
		odom.twist.twist.linear.y = vy;
		odom.twist.twist.angular.z = vth;
		odom.twist.covariance = {1e-3, 0, 0, 0, 0, 0,
								 0, 1e-3, 0, 0, 0, 0,
								 0, 0, 1e6, 0, 0, 0,
								 0, 0, 0, 1e6, 0, 0,
								 0, 0, 0, 0, 1e6, 0,
								 0, 0, 0, 0, 0, 1e3};

		// publish the message
		odom_pub.publish(odom);

		// pub imu data
		double delta_wz = wz * dt;
		ang_wz += delta_wz;

		// 2d robot,calc quaternion from yaw
		geometry_msgs::Quaternion imu_quat = tf::createQuaternionMsgFromYaw(ang_wz);
		sensor_msgs::Imu imu_data;

		imu_data.header.stamp = ros::Time::now();
		imu_data.header.frame_id = "imu_data";

		imu_data.angular_velocity.x = 0;
		imu_data.angular_velocity.y = 0;
		imu_data.angular_velocity.z = wz;
		imu_data.angular_velocity_covariance = {1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6};

		imu_data.orientation = imu_quat;
		imu_data.orientation_covariance = {1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6};

		imu_data.linear_acceleration.x = ax;
		imu_data.linear_acceleration.y = ay;
		imu_data.linear_acceleration.z = 0;
		imu_data.linear_acceleration_covariance = {-1, 0, 0, 0, 0, 0, 0, 0, 0};

		imu_pub.publish(imu_data);

		last_time = current_time;
		r.sleep();
	}
}
